#include "GoCurveActionGroup.h"

GoCurveActionGroup::GoCurveActionGroup(ArRobot* robot, int speed,
		uint16_t distanceToCurve, double turningTime, int rotVel, int rotDecel) :
		ArActionGroup(robot) {
	this->turningTime = turningTime;
	this->speed = speed;
	mustCurve = false;
	emergencyStop = false;
	curve = new CurveAction(robot, turningTime, rotVel, rotDecel, &mustCurve);
	advance = new GoStraightAction(robot, speed, &mustCurve, distanceToCurve,
			&emergencyStop);
	addAction(curve, 50);
	addAction(advance, 50);
}

GoCurveActionGroup::~GoCurveActionGroup() {
	delete curve;
	delete advance;
}

bool GoCurveActionGroup::haveAchievedDistance() {
	return advance->haveAchievedDistance() && curve->haveAchievedCurve();
}

void GoCurveActionGroup::setDistance(double totalDistance, int direction) {

	advance->setDistance(totalDistance);
	curve->setDeltaHeading(this->turningTime, direction);
	emergencyStop = false;

}

void GoCurveActionGroup::stop() {
	emergencyStop = true;
}

bool GoCurveActionGroup::isStartedCurve() {
	return mustCurve;
}
